#include "RobotController.h"

RobotController::RobotController(ArRobot* robot) {
	initRobotParameters();
	this->robot = robot;
	direction = 0;
	vision = new VisionControllerBW(stackedBoxHorizontalTolerance,
			stackedBoxVerticalTolerance, maxGhostFrames, &epsilonFromCenter,
			&epsilonStabilization);
	followRotVel = followVMin;
	followMarkerState = new FollowMarkerActionGroup(robot, &direction,
			followRotVel, &distance, (uint16_t) distanceSetPoint,
			(uint16_t) followDMax, followVMax, followVMin);
	goCurveState = new GoCurveActionGroup(robot, curveTransVel,
			distanceToCurve + distanceToCurve / 2, curveTurningTime,
			curveRotVel, curveRotDecel);
	searchState = new SearchActionGroup(robot, direction, searchRotVel);
	obstacleAvoidanceState = new ObstacleAvoidanceActionGroup(robot,
			obstacleTransVel, obstacleRotVel, obstacleAvoidingDistanceToTravel,
			obstacleTurningTime);
	searchContinuoslyState = new SearchContinuoslyGroup(robot, direction,
			searchRotVel);
	myState = FOLLOW_MARKER;
	followMarkerState->activateExclusive();
	stoppedStartTime = cv::getTickCount();
	lastValidDirection = 0;
}

void RobotController::initRobotParameters() {

	distanceToAdvanceInCaseOfObstacle = vision->DEPTHMIN / 3;
	safetyFactorInCaseOfObstacle = 1.4;

	/*
	 * Parametri di VisionController
	 */
	maxGhostFrames = 3;
	stackedBoxHorizontalTolerance = 0.5;
	stackedBoxVerticalTolerance = 2;
	//distanza dal centro per considerare fuoriuscita marker
	strictEpsilon = 130; //150
	largeEpsilon = 320;
	epsilonFromCenter = strictEpsilon;

	//distanza dal centro da considerare nella quale stabilizzare l'orientamento del robot rispetto al marker
	epsilonStabilization = strictEpsilon / 2;

	/*
	 * 	 Parametri della Search
	 *
	 */
	minRotVelToConsiderStoppedTheRobot = 2;
	searchRotVel = 50;
	searchTimeInterval = 1;
	searchHeadingInterval = 25;
	searchRotDecel = 300;

	/*
	 * Parametri della FollowMarker
	 */
	distanceSetPoint = 300;		//800
	followTransVel = 1000;		//2000
	followVMin = 700;
	followVMax = 2000;
	followDMax = 1300;
	//700-233
	//800- 267
	//1000-333
	weakDecel = 200;	//600-
	strongDecel = 800;	//1200
	fastRotVel = 40;		//20
	slowRotVel = 18;		//18
	followRotVel = slowRotVel;
	fastRotAcc = 20;
	slowRotAcc = 10;
	stoppedTime = 5;

	/*
	 * Parametri della FollowTraj
	 */
	curveTransVel = followTransVel;
	curveRotVel = searchRotVel;
	curveTurningTime = 1.2;
	curveRotDecel = searchRotDecel;
	distanceToCurve = 200;
	/**
	 * Parametri dell'obstacle avoidance
	 *
	 */
	minDistanceToObstacle = 800;
	obstacleRotVel = 40;
	obstacleAvoidingDistanceToTravel = 400;
	obstacleTransVel = 400;
	obstacleTurningTime = 0.7;
	minDiffMarkerFromObstacle = 250;

}

int RobotController::checkObstacles(vector<uint16_t>* left,
		vector<uint16_t>* right) {
	int minLeftIndex = 0, minRightIndex = 0;
	for (int i = 1; i < left->size(); i++) {
		if (left->at(i) < left->at(minLeftIndex))
			minLeftIndex = i;
	}
	for (int i = 1; i < right->size(); i++) {
		if (right->at(i) < right->at(minRightIndex))
			minRightIndex = i;
	}

	if (left->at(minLeftIndex) < right->at(minRightIndex)) {
		if (left->at(minLeftIndex) < minDistanceToObstacle)
			return (minLeftIndex * (-1)) - 1;
	} else {
		if (right->at(minRightIndex) < minDistanceToObstacle)
			return minRightIndex + 1;
	}

	return 0;
}

uint16_t RobotController::getMarkerDistanceFromObstacle(vector<uint16_t>* right,
		vector<uint16_t>* left, int obstacleLabel, uint16_t markerDistance) {
	uint16_t diff = 0;
	if (obstacleLabel != 0) {
		if (obstacleLabel > 0)
			diff = abs(right->at(obstacleLabel - 1) - markerDistance);
		else {
			int obstacleIndex = (obstacleLabel * (-1)) - 1;
			diff = abs(left->at(obstacleIndex) - markerDistance);
		}

	}
	return diff;
}

void RobotController::run() {
	running = true;
	pthread_create(&thread, 0, &RobotController::start, this);
}

void RobotController::stop() {
	stop_request = true;
	running = false;
	pthread_join(thread, 0);
}

void RobotController::do_work() {
	Target* target = new Target();
	Target* lastTarget = new Target();
	int trajPointDistance;
	bool initTrajState = true;
	bool initSearch = true;
	bool initObstacle = true;
	int obstacleLabel = 0;
	bool searchContinuosly = false;
	vector<uint16_t>* right = new vector<uint16_t>();
	vector<uint16_t>*left = new vector<uint16_t>();
	uint16_t diffMarkerFromObstacle = 0;
	std::stringstream ss;
	cout << "inizio " << endl;
	while (!stop_request) {
		//	cout << "vel " << robot->getVel() << endl;
		//cout << "epsFromCen" << epsilonFromCenter << endl;
//		cout << "a"<<endl;

		Target *newTarget = vision->processFrame();
		*target = *newTarget;
//		cout<<"target: "<<abs(target->getDistanceFromCenter())<<endl;
		cout << "vel: " << robot->getVel() <<"\t" << "rotVel: "<<robot->getRotVel()<< endl;

		//cout<<"last target 2: "<<abs(lastTarget->getDistanceFromCenter())<<endl;
//		cout << "b"<<endl;
		vision->drawLineEpsilon(epsilonFromCenter);
//		cout << "c"<<endl;
		distance = target->getDistance();
//		cout << "d"<<endl;
		// Aggiorno la distanza dell'ultimo target valido trovato

		if (lastTarget->getDistance() > 0)
			lastValidDistance = lastTarget->getDistance();
//		cout << "e"<<endl;
		int distanceFromCenter;
		switch (myState) {
		case FOLLOW_MARKER:
			cout << "follow " << endl;
			//i primi elementi sono quelli più vicini al centro

			followMarkerState->activateExclusive();
			//impostazione direzione
			distanceFromCenter = target->getDistanceFromCenter();
			if (abs(distanceFromCenter) > epsilonStabilization) {
				/*
				 * Modifico la velocità di rotazione in base alla distanza dal centro
				 */
				int rotVel = (double)(
						abs(distanceFromCenter) - epsilonStabilization)
						* ((double) (fastRotVel - slowRotVel))
						/ ((double) (largeEpsilon - epsilonStabilization))+slowRotVel;
				cout<<"rotVel desired: "<<rotVel<<endl;
				if (target->getDistanceFromCenter() > 0)
					direction = -1;
				else
					direction = 1;
				followMarkerState->setRotVel(rotVel,&direction);

			} else {

				direction = 0;
				//fase di riallineamento
				if (abs(robot->getRotVel()) < minRotVelToConsiderStoppedTheRobot
						&& distance > 0) {
					epsilonFromCenter = strictEpsilon;

				}		//aggiornamento direction con marker perso

				//direction = lastTarget->distanceFromCenter > 0 ? -1 : 1;

			}

			/*
			 * Regolazione velocità in base a distanza
			 */

			//GESTIONE OBSTACLE AVOIDANCE
			//non posso avere (in base alla dist.minima regolamentare degli ostacoli)
			//un ostacolo sotto soglia sia in right che in left
			delete right;
			delete left;

			right = vision->getRightObstaclesDistances();
			left = vision->getLeftObstaclesDistances();
			obstacleLabel = checkObstacles(left, right);
			diffMarkerFromObstacle = getMarkerDistanceFromObstacle(right, left,
					obstacleLabel, distance);
			//in caso di presenza di un ostacolo, cambio stato

			if (obstacleLabel != 0 && direction == 0
					&& target->getDistance() > distanceSetPoint / 2
					&& robot->getVel() > 0
					&& diffMarkerFromObstacle >= minDiffMarkerFromObstacle
					&& vision->getFrontalDistance()
							> minDistanceToObstacle + 400 && false) {
				//prima di andare in ostacolo, verifico presenza ostacoli
				//dall'altro lato
				uint16_t distOtherObstacle = vision->DEPTHMAX;
				if (obstacleLabel > 0)
					distOtherObstacle = getMinValue(left->at(0), left->at(1));
				else
					distOtherObstacle = getMinValue(right->at(0), right->at(1));

				if (distOtherObstacle <= minDistanceToObstacle + 400) {
					epsilonFromCenter = largeEpsilon;
					myState = AVOIDANCE;
				}
			}				//endif obstacleLabel != 0
			if (myState != AVOIDANCE) {	//in assenza di ostacoli, followMarker

				//gestione decelerazione
				if (target->getDistance() < (double)distanceSetPoint*1.5 && abs(target->getDistanceFromCenter())<epsilonStabilization)
					followMarkerState->setDecel(strongDecel);
				else
					followMarkerState->setDecel(weakDecel);

				//cout << "direction " << direction << endl;
				//cambio di stato (trajectory)
//				if (abs(target->getDistanceFromCenter()) > epsilonFromCenter
//						&& abs(lastTarget->getDistanceFromCenter())
//								<= epsilonFromCenter
//						&& lastTarget->getDistance() > 0
//						&& target->getDistance() > (distanceSetPoint * 7)) {
//					trajPointDistance = distance - 100;
//					myState = FOLLOW_TRAJ;
//					cout << "vado in curva" << endl;
//
//				}

				if (abs(lastTarget->getDistanceFromCenter()) > epsilonStabilization
						&& !target->isValid()) {

					if (lastTarget->getDistanceFromCenter() > 0)
						direction = -1;
					else
						direction = 1;

					epsilonFromCenter = largeEpsilon;
					myState = SEARCH;
					cout << "vado in search 2: " << direction << endl;
				}
//				if (abs(target->getDistanceFromCenter()) > epsilonFromCenter
//						&& abs(lastTarget->getDistanceFromCenter())
//								<= epsilonFromCenter
//						&& lastTarget->getDistance() > 0
//						&& target->getDistance() <= (distanceSetPoint / 2)) {
//					epsilonFromCenter = largeEpsilon;
//					myState = SEARCH;
//					cout << "vado in search" << endl;
//
//				}
				//search continuosly se rimango fermo senza vedere il marker per n secondi
				if (!target->isValid() && robot->getVel() == 0) {
					double elapsed = (cv::getTickCount() - stoppedStartTime)
							/ cv::getTickFrequency();
					if (elapsed >= stoppedTime) {
						myState = SEARCH;
						searchContinuosly = true;
						direction=lastValidDirection;
						if (direction == 0)
							direction = 1;
					}
				} else
					stoppedStartTime = cv::getTickCount();

				//aggiornamento last valid direction
				if(target->isValid())
						lastValidDirection = direction;
			}

			break;
		case FOLLOW_TRAJ:
			cout << "traj " << endl;
			// controllo se è l'istante di cambio stato
			if (initTrajState) {
				// setto la distanza alla quale dovrà andare il robot
				goCurveState->setDistance(trajPointDistance - 100, direction);
				initTrajState = false;
				// Attivo l'actionGroup
				goCurveState->activateExclusive();

			}

			if (goCurveState->isStartedCurve()) {
				epsilonFromCenter = largeEpsilon;

			}

			// Primo cambio di stato: ho raggiunto la distanza e devo andare nello stato Search
			if (goCurveState->haveAchievedDistance()) {
				myState = SEARCH;
				initTrajState = true;
			}
			// controllo se mi sto avvicinando troppo ad un ostacolo
			else {
				uint16_t frontalDistance = vision->getFrontalDistance();
				cout << "front distance " << frontalDistance << endl;

				//se entro nella minima distanza misurabile, verifico di non avanzare troppo rispetto ad un eventuale ostacolo
				if ((frontalDistance > vision->DEPTHMIN)
						&& frontalDistance
								< (uint16_t) ((float) (vision->DEPTHMIN)
										* safetyFactorInCaseOfObstacle)) {
					//					goCurveState->setDistance(
					//							distanceToAdvanceInCaseOfObstacle);
					goCurveState->stop();
					cout << "Stop distanza min" << endl;

				}
			}

			//marker rientrato
			if (target->getDistanceFromCenter() != 0
					&& target->getDistance() != 0
					&& abs(target->getDistanceFromCenter())
							< epsilonStabilization) {
				myState = FOLLOW_MARKER;
				initTrajState = true;
				cout << "marker rientrato" << endl;
			}

			break;
		case SEARCH:
			cout << "search " << endl;
			//cout << "search" << endl;

			ss << direction;
			//cout << ss.str() << endl;
			if (initSearch) {
				searchState->restart(direction);
				searchContinuoslyState->restart(direction);
				initSearch = false;
			}
			if (searchContinuosly)
				searchContinuoslyState->activateExclusive();
			else
				searchState->activateExclusive();
			if (target->getDistanceFromCenter() != 0
					&& target->getDistance() != 0
					&& abs(target->getDistanceFromCenter())
							< epsilonFromCenter) {
				myState = FOLLOW_MARKER;
				initSearch = true;
				searchContinuosly = false;

			}
			break;
		case AVOIDANCE:
			cout << "AVOIDANCE" << endl;
			if (initObstacle) {

				int startingDirection = 0;
				//ostacolo right
				if (obstacleLabel > 0)
					startingDirection = leftDirection;
				else
					startingDirection = rightDirection;

				obstacleAvoidanceState->start(startingDirection,
						minDistanceToObstacle, obstacleTurningTime);
				obstacleAvoidanceState->activateExclusive();
				initObstacle = false;
			} else if (obstacleAvoidanceState->hasCompletedAvoidance()) {
				initObstacle = true;
				myState = FOLLOW_MARKER;
			}

			break;

		}

		*lastTarget = *target;

	}
	delete right;
	delete left;
	delete target;
	delete lastTarget;

}

RobotController::~RobotController() {
	delete followMarkerState;
	delete goCurveState;
	delete searchState;
	delete vision;
}

uint16_t RobotController::getMinValue(uint16_t a, uint16_t b) {
	if (a < b)
		return a;
	else
		return b;
}

